#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <sensor_msgs/Image.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>

#include <map>
#include "XmlParser.h"

using namespace cv;
using namespace std;

// openCV objects
namespace enc = sensor_msgs::image_encodings;
static const char WINDOW[] = "Image window";

// parameters parsed from params.xml
map<string, string> params;

// minimal contour (in pixels) for an object
int min_obj_size;

// lower and upper bounds of color to detect
int lower_h;
int lower_s;
int lower_v;
int upper_h;
int upper_s;
int upper_v;

/**
 * Displays a binary image, where white pixels match the contours of objects
 * that their color match the bounds parsed from params.xml
 */
class ColorDetector {
	ros::NodeHandle nh_;
	image_transport::ImageTransport it_;
	image_transport::Subscriber image_sub_;
	image_transport::Publisher image_pub_;

public:

	/**
	 * Constructor
	 */
	ColorDetector() :
			it_(nh_) {
		image_pub_ = it_.advertise("out", 1);

		// the kinect
		image_sub_ = it_.subscribe("/camera/rgb/image_color", 1, &ColorDetector::imageCb, this);

		// the webcam
		// image_sub_ = it_.subscribe("/image_raw", 1, &ImageConverter::imageCb, this); // if wants to calibrate according to webcam

		cv::namedWindow(WINDOW);
	}

	/**
	 * Destructor
	 */
	~ColorDetector() {
		cv::destroyWindow(WINDOW);
	}

	/**
	 * Callback for the topic published by the relevant camera (Kinect or webcam).
	 * Updates the displayed binary image
	 * @param msg holds the published image
	 */
	void imageCb(const sensor_msgs::ImageConstPtr& msg) {

		// convert published data to openCV image
		cv_bridge::CvImagePtr cv_ptr;
		try {
			cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
		} catch (cv_bridge::Exception& e) {
			ROS_ERROR("cv_bridge exception: %s", e.what());
			return;
		}

		// perform thresholding
		IplImage input(cv_ptr->image);
		IplImage* img = cvCloneImage(&input);
		IplImage* imgHSV = cvCreateImage(cvGetSize(img), 8, 3);
		cvCvtColor(img, imgHSV, CV_BGR2HSV);
		IplImage* imgThreshed = cvCreateImage(cvGetSize(img), 8, 1);
		cvInRangeS(imgHSV, cvScalar(lower_h, lower_s, lower_v),
						   cvScalar(upper_h, upper_s, upper_v), imgThreshed);
		RNG rng(12345);

		// read the image
		Mat src = Mat(imgThreshed, true);
		Mat drawing = Mat::zeros(src.size(), CV_8UC3);

		// find contours
		std::vector<std::vector<cv::Point> > contours;
		std::vector<cv::Vec4i> hierarchy;
		cv::findContours(src, contours, hierarchy, CV_RETR_TREE,
				CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0));

		// get the moments

		double area = 0.0;
		double max_area = 0.0;
		int max_index = -1;

		for (unsigned int i = 0; i < contours.size(); ++i) {

			area = contourArea(contours[i]);

			if (area <= min_obj_size) {
				continue;
			}

			if (area > max_area) {
				max_area = area;
				max_index = i;
			}
		}

		if (max_index < 0) { // could not find a significant contour

			ROS_ERROR("max_index = %d", max_index);

		} else { // found a contour

			Moments mu = moments(contours[max_index], false);
			Point2f p2f = Point2f(mu.m10 / mu.m00, mu.m01 / mu.m00);

			// drawContours( drawing, contours, max_index, cv::Scalar(255,0,0), 2, 8, hierarchy, 0, Point() ); // print contours

			// draw a circle around detected object's center
			circle(src, p2f, 30, Scalar(255, 0, 0), 3, 8, 0);

			// print center's position in image
			ROS_INFO("p2f = [x,y] = [%d,%d]", int(p2f.x), int(p2f.y));

		}

		// update display
		cv::imshow(WINDOW, src);

		// release memory
		cvReleaseImage(&img);
		cvReleaseImage(&imgHSV);
		cvReleaseImage(&imgThreshed);

		cv::waitKey(3);

		image_pub_.publish(cv_ptr->toImageMsg());
	}
};

/**
 * Parses threshold value of color to detect out of params.xml
 * @param min_obj_size[out] minimal contour (in pixels) for an object
 * @param h1[out] Hue for lower bound
 * @param s1[out] Saturation for lower bound
 * @param v1[out] Value for lower bound
 * @param h2[out] Hue for upper bound
 * @param s2[out] Saturation for upper bound
 * @param v2[out] Value for upper bound
 */
void parseParams(int& min_obj_size, int& h1, int& s1, int& v1, int& h2, int& s2,
		int& v2) {

	min_obj_size = atoi(params["min_obj_size"].c_str());
	h1 = atoi(params["lower_h"].c_str());
	s1 = atoi(params["lower_s"].c_str());
	v1 = atoi(params["lower_v"].c_str());
	h2 = atoi(params["upper_h"].c_str());
	s2 = atoi(params["upper_s"].c_str());
	v2 = atoi(params["upper_v"].c_str());
}

int main(int argc, char** argv) throw (const char*) {

	ros::init(argc, argv, "ColorCalibration");

	// reading the parameters
	XmlParser::extractNodeData("ColorCalibration", params);

	parseParams(min_obj_size, lower_h, lower_s, lower_v, upper_h, upper_s, upper_v);

	ColorDetector ic;
	ros::spin();
	return 0;
}
